#include <test_vgicp.h>
#include <chrono>
#include <pcl/registration/gicp.h>
#include <pcl/io/pcd_io.h>

#include<iostream>
#include <pcl/filters/approximate_voxel_grid.h>

#include <fast_gicp/gicp/fast_vgicp.hpp>

//#ifdef USE_VGICP_CUDA
#include <fast_gicp/ndt/ndt_cuda.hpp>
#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
//#endif

//   // remove invalid points around origin
//   source_cloud->erase(
//     std::remove_if(source_cloud->begin(), source_cloud->end(), [=](const pcl::PointXYZ& pt) { return pt.getVector3fMap().squaredNorm() < 1e-3; }),
//     source_cloud->end());
//   target_cloud->erase(
//     std::remove_if(target_cloud->begin(), target_cloud->end(), [=](const pcl::PointXYZ& pt) { return pt.getVector3fMap().squaredNorm() < 1e-3; }),
//     target_cloud->end());

char* get_file_suffix(char*filename)
{
    int len =strlen(filename);
    int i=0;
    for(i=0;i<len;i++)
    {
        if(filename[len-i-1]=='.')
        {
            break;
        }
    }

    if(i==0)
    {
        return NULL;
    }

    char* re=(char*)malloc(sizeof(char)*(i+1) );
    memset(re,0,sizeof(char)*(i+1));

    for(int j=0;j<i;j++)
    {
        re[j]= filename[len-i+j ];
    }


    return re;
}




float * cuda_vgicp_align( char* source_file_name,char* target_file_name,double* mat1 )
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());


    if (pcl::io::loadPCDFile(target_file_name, *target_cloud)) {
        std::cerr << "failed to open " << target_file_name << std::endl;
        return NULL;
    }
    if (pcl::io::loadPCDFile(source_file_name, *source_cloud)) {
        std::cerr << "failed to open " << source_file_name << std::endl;
        return NULL;
    }

    fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ> vgicp_cuda;
    vgicp_cuda.setResolution(1.0);//0.5
    vgicp_cuda.setMaximumIterations(150);
    vgicp_cuda.lm_max_iterations_=140;
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);

    double fitness_score = 0.0;

    auto t1 = std::chrono::high_resolution_clock::now();
  
    vgicp_cuda.clearTarget();
    vgicp_cuda.clearSource();


    vgicp_cuda.setInputTarget(target_cloud);
    vgicp_cuda.setInputSource(source_cloud);
    vgicp_cuda.align(*aligned);
    auto mat=vgicp_cuda.getFinalTransformation();

    float * re=(float*)malloc(sizeof(float)*16);
    memmove(re,mat.data(),sizeof(float)*16);

    printf("end align\n");
    //std::cout<<mat<<std::endl;
       //getFinalTransformation
    //std::cout << "single:" << single << "[msec] " << std::flush;

    return re;

}



float * cuda_ndt_align( char* source_file_name,char* target_file_name,double* mat1 )
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());


    if (pcl::io::loadPCDFile(target_file_name, *target_cloud)) {
        std::cerr << "failed to open " << target_file_name << std::endl;
        return NULL;
    }
    if (pcl::io::loadPCDFile(source_file_name, *source_cloud)) 
    {
        std::cerr << "failed to open " << source_file_name << std::endl;
        return NULL;
    }

//
    Eigen::Matrix<float,4,4> guess_mat;
    if(mat1!=NULL)
    {
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                guess_mat.coeffRef(i,j)=mat1[i*4+j];
            }
        }
    }
    
//   // downsampling  

    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxelgrid;
    voxelgrid.setLeafSize(0.15f, 0.15f, 0.15f);

    voxelgrid.setInputCloud(target_cloud);
    voxelgrid.filter(*filtered);
    target_cloud = filtered;

//   filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
//   voxelgrid.setInputCloud(source_cloud);
//   voxelgrid.filter(*filtered);
//   source_cloud = filtered;




    fast_gicp::NDTCuda<pcl::PointXYZ, pcl::PointXYZ> ndt_cuda;
    ndt_cuda.setResolution(0.5);//0.5
    ndt_cuda.setDistanceMode(fast_gicp::NDTDistanceMode::D2D);
    

    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);

    double fitness_score = 0.0;

    auto t1 = std::chrono::high_resolution_clock::now();
  
    ndt_cuda.clearTarget();
    ndt_cuda.clearSource();
    ndt_cuda.setMaximumIterations(250);
    ndt_cuda.lm_max_iterations_=240;

    ndt_cuda.setInputTarget(target_cloud);
    ndt_cuda.setInputSource(source_cloud);
    if(mat1!=NULL)
    {
        ndt_cuda.align(*aligned,guess_mat);
    }
    else
    {
        ndt_cuda.align(*aligned);
    }
    auto mat=ndt_cuda.getFinalTransformation();

    float * re=(float*)malloc(sizeof(float)*16);
    memmove(re,mat.data(),sizeof(float)*16);
    
    printf("end align\n");
    std::cout<<mat<<std::endl;
       //getFinalTransformation
    //std::cout << "single:" << single << "[msec] " << std::flush;

    return re;

}







